#include "stdafx.h"

#include "bochs.h"

extern float_status_t FPU_pre_exception_handling(Bit16u control_word);

#include "softfloatx80.h"

void IA32_CPU::FLD_STi(Ia32_Instruction_c *i)
{
   prepareFPU(i);

  clear_C1();

  if (! IS_TAG_EMPTY(-1))
  {
       FPU_stack_overflow();
      return; 
  }

  if (IS_TAG_EMPTY(i->rm()))
  {
      FPU_stack_underflow(0);
     return;
  }

  int sti_tag =  the_i387.FPU_gettagi(i->rm());
  floatx80 sti_reg = IA32_READ_FPU_REG(i->rm());

   the_i387.FPU_push();
  IA32_WRITE_FPU_REGISTER_AND_TAG(sti_reg, sti_tag, 0);
}

void IA32_CPU::FLD_SINGLE_REAL(Ia32_Instruction_c *i)
{
   prepareFPU(i);

  clear_C1();

  if (! IS_TAG_EMPTY(-1))
  {
       FPU_stack_overflow();
      return; 
  }

  float32 load_reg;
  read_virtual_dword(i->seg(), IA32_RMAddr(i), &load_reg);

  float_status_t status = 
     FPU_pre_exception_handling( the_i387.get_control_word());

  // convert to floatx80 format
  floatx80 result = float32_to_floatx80(load_reg, status);

  if ( FPU_exception(status.float_exception_flags))
     return;

   the_i387.FPU_push();
  IA32_WRITE_FPU_REG(result, 0);
}

void IA32_CPU::FLD_DOUBLE_REAL(Ia32_Instruction_c *i)
{
   prepareFPU(i);

  clear_C1();

  if (! IS_TAG_EMPTY(-1))
  {
       FPU_stack_overflow();
      return; 
  }

  float64 load_reg;
  read_virtual_qword(i->seg(), IA32_RMAddr(i), &load_reg);

  float_status_t status = 
     FPU_pre_exception_handling( the_i387.get_control_word());

  // convert to floatx80 format
  floatx80 result = float64_to_floatx80(load_reg, status);

  if ( FPU_exception(status.float_exception_flags))
     return;

   the_i387.FPU_push();
  IA32_WRITE_FPU_REG(result, 0);
}

void IA32_CPU::FLD_EXTENDED_REAL(Ia32_Instruction_c *i)
{
   prepareFPU(i);

  clear_C1();

  if (! IS_TAG_EMPTY(-1))
  {
       FPU_stack_overflow();
      return; 
  }

  floatx80 result;
  read_virtual_tword(i->seg(), IA32_RMAddr(i), &result);

   the_i387.FPU_push();
  IA32_WRITE_FPU_REG(result, 0);
}

/* DF /0 */
void IA32_CPU::FILD_WORD_INTEGER(Ia32_Instruction_c *i)
{
   prepareFPU(i);

  clear_C1();

  if (! IS_TAG_EMPTY(-1))
  {
       FPU_stack_overflow();
      return; 
  }

  Bit16s load_reg;
  read_virtual_word(i->seg(), IA32_RMAddr(i), (Bit16u*)(&load_reg));
  floatx80 result = int32_to_floatx80((Bit32s) load_reg);

   the_i387.FPU_push();
  IA32_WRITE_FPU_REG(result, 0);
}

/* DB /0 */
void IA32_CPU::FILD_DWORD_INTEGER(Ia32_Instruction_c *i)
{
   prepareFPU(i);

  clear_C1();

  if (! IS_TAG_EMPTY(-1))
  {
       FPU_stack_overflow();
      return; 
  }

  Bit32s load_reg;
  read_virtual_dword(i->seg(), IA32_RMAddr(i), (Bit32u*)(&load_reg));
  floatx80 result = int32_to_floatx80(load_reg);

   the_i387.FPU_push();
  IA32_WRITE_FPU_REG(result, 0);
}

/* DF /5 */
void IA32_CPU::FILD_QWORD_INTEGER(Ia32_Instruction_c *i)
{
   prepareFPU(i);

  clear_C1();

  if (! IS_TAG_EMPTY(-1))
  {
       FPU_stack_overflow();
      return; 
  }

  Bit64s load_reg;
  read_virtual_qword(i->seg(), IA32_RMAddr(i), (Bit64u*)(&load_reg));
  floatx80 result = int64_to_floatx80(load_reg);

   the_i387.FPU_push();
  IA32_WRITE_FPU_REG(result, 0);
}

/* DF /4 */
void IA32_CPU::FBLD_PACKED_BCD(Ia32_Instruction_c *i)
{
   prepareFPU(i);

  clear_C1();

  if (! IS_TAG_EMPTY(-1))
  {
       FPU_stack_overflow();
      return; 
  }

  Bit16u hi2;
  Bit64u lo8;

  // read packed bcd from memory
  read_virtual_qword(i->seg(), IA32_RMAddr(i),     &lo8);
  read_virtual_word (i->seg(), IA32_RMAddr(i) + 8, &hi2);

  Bit64s scale = 1; 
  Bit64s val64 = 0;

  for (int n = 0; n < 16; n++)
  {
    val64 += (lo8 & 0x0F) * scale;
    lo8 >>= 4;
    scale *= 10;
  }

  val64 += (hi2 & 0x0F) * scale;
  val64 += ((hi2>>4) & 0x0F) * scale * 10;

  floatx80 result = int64_to_floatx80(val64);
  if (hi2 & 0x8000)	// set negative
      floatx80_chs(result);

   the_i387.FPU_push();
  IA32_WRITE_FPU_REG(result, 0);
}

void IA32_CPU::FST_STi(Ia32_Instruction_c *i)
{
   prepareFPU(i);

  int pop_stack = i->nnn() & 1;

  int st0_tag =  the_i387.FPU_gettagi(0);
  if (st0_tag == FPU_Tag_Empty)
  {
      FPU_stack_underflow(i->rm(), pop_stack);
     return;
  }

  floatx80 st0_reg = IA32_READ_FPU_REG(0);

  IA32_WRITE_FPU_REGISTER_AND_TAG(st0_reg, st0_tag, i->rm());
  if (pop_stack)
      the_i387.FPU_pop();
}

void IA32_CPU::FST_SINGLE_REAL(Ia32_Instruction_c *i)
{
   prepareFPU(i);

  float32 save_reg = float32_default_nan; /* The masked response */

  int pop_stack = i->nnn() & 1;

  if (IS_TAG_EMPTY(0))
  {
      FPU_exception(FPU_EX_Stack_Underflow);

     if (! ( the_i387.is_IA_masked()))
        return;
  }
  else
  {
     float_status_t status = 
         FPU_pre_exception_handling( the_i387.get_control_word());

     save_reg = floatx80_to_float32(IA32_READ_FPU_REG(0), status);

     if ( FPU_exception(status.float_exception_flags))
        return;
  }

  write_virtual_dword(i->seg(), IA32_RMAddr(i), &save_reg);

  if (pop_stack)
      the_i387.FPU_pop();
}

void IA32_CPU::FST_DOUBLE_REAL(Ia32_Instruction_c *i)
{
   prepareFPU(i);

  float64 save_reg = float64_default_nan; /* The masked response */

  int pop_stack = i->nnn() & 1;

  if (IS_TAG_EMPTY(0))
  {
      FPU_exception(FPU_EX_Stack_Underflow);

     if (! ( the_i387.is_IA_masked()))
        return;
  }
  else
  {
     float_status_t status = 
         FPU_pre_exception_handling( the_i387.get_control_word());

     save_reg = floatx80_to_float64(IA32_READ_FPU_REG(0), status);

     if ( FPU_exception(status.float_exception_flags))
        return;
  }

  write_virtual_qword(i->seg(), IA32_RMAddr(i), &save_reg);

  if (pop_stack)
      the_i387.FPU_pop();
}

/* DB /7 */
void IA32_CPU::FSTP_EXTENDED_REAL(Ia32_Instruction_c *i)
{
   prepareFPU(i);

  floatx80 save_reg = floatx80_default_nan; /* The masked response */

  if (IS_TAG_EMPTY(0))
  {
      FPU_exception(FPU_EX_Stack_Underflow);

     if (! ( the_i387.is_IA_masked()))
        return;
  }
  else
  {
     save_reg = IA32_READ_FPU_REG(0);
  }

  write_virtual_tword(i->seg(), IA32_RMAddr(i), &save_reg);

   the_i387.FPU_pop();
}

void IA32_CPU::FIST_WORD_INTEGER(Ia32_Instruction_c *i)
{
   prepareFPU(i);

  Bit16s save_reg = int16_indefinite;

  int pop_stack = i->nnn() & 1;

  clear_C1();

  if (IS_TAG_EMPTY(0))
  {
      FPU_exception(FPU_EX_Stack_Underflow);

     if (! ( the_i387.is_IA_masked()))
        return;
  }
  else
  {
      float_status_t status = 
         FPU_pre_exception_handling( the_i387.get_control_word());

      save_reg = floatx80_to_int16(IA32_READ_FPU_REG(0), status);

      if ( FPU_exception(status.float_exception_flags))
        return;
  }

  write_virtual_word(i->seg(), IA32_RMAddr(i), (Bit16u*)(&save_reg));

  if (pop_stack)
      the_i387.FPU_pop();
}

void IA32_CPU::FIST_DWORD_INTEGER(Ia32_Instruction_c *i)
{
   prepareFPU(i);

  Bit32s save_reg = int32_indefinite; /* The masked response */

  int pop_stack = i->nnn() & 1;

  clear_C1();

  if (IS_TAG_EMPTY(0))
  {
      FPU_exception(FPU_EX_Stack_Underflow);

     if (! ( the_i387.is_IA_masked()))
        return;
  }
  else
  {
     float_status_t status = 
         FPU_pre_exception_handling( the_i387.get_control_word());

     save_reg = floatx80_to_int32(IA32_READ_FPU_REG(0), status);

     if ( FPU_exception(status.float_exception_flags))
         return;
  }

  write_virtual_dword(i->seg(), IA32_RMAddr(i), (Bit32u*)(&save_reg));

  if (pop_stack)
      the_i387.FPU_pop();
}

void IA32_CPU::FISTP_QWORD_INTEGER(Ia32_Instruction_c *i)
{
   prepareFPU(i);

  Bit64s save_reg = int64_indefinite; /* The masked response */

  clear_C1();

  if (IS_TAG_EMPTY(0))
  {
      FPU_exception(FPU_EX_Stack_Underflow);

     if (! ( the_i387.is_IA_masked()))
        return;
  }
  else
  {
     float_status_t status = 
         FPU_pre_exception_handling( the_i387.get_control_word());

     save_reg = floatx80_to_int64(IA32_READ_FPU_REG(0), status);

     if ( FPU_exception(status.float_exception_flags))
         return;
  }

  write_virtual_qword(i->seg(), IA32_RMAddr(i), (Bit64u*)(&save_reg));
   the_i387.FPU_pop();
}

void IA32_CPU::FBSTP_PACKED_BCD(Ia32_Instruction_c *i)
{
   prepareFPU(i);

  Bit16u save_reg_hi = 0xFFFF;
  Bit64u save_reg_lo = IA32_CONST64(0xC000000000000000LL);

  clear_C1();

  if (IS_TAG_EMPTY(0))
  {
      FPU_exception(FPU_EX_Stack_Underflow);

     if (! ( the_i387.is_IA_masked()))
        return;
  }
  else
  {
     float_status_t status = 
        FPU_pre_exception_handling( the_i387.get_control_word());

     Bit64s save_val = floatx80_to_int64(IA32_READ_FPU_REG(0), status);

     int sign = (save_val < 0);
     if (sign) 
        save_val = -save_val;
 
     if (save_val > IA32_CONST64(999999999999999999LL))
     {
        float_raise(status, float_flag_invalid);
     }
     
     if (! (status.float_exception_flags & float_flag_invalid))
     {
        save_reg_hi = (sign) ? 0x8000 : 0;
        save_reg_lo = 0;

        for (int i=0; i<16; i++)
        {
           save_reg_lo += ((Bit64u)(save_val % 10)) << (4*i);
           save_val /= 10;
        }

        save_reg_hi += (Bit16u)(save_val % 10);
        save_val /= 10;
        save_reg_hi += (Bit16u)(save_val % 10) << 4;
    }

    /* check for fpu arithmetic exceptions */
    if ( FPU_exception(status.float_exception_flags))
        return;
  }

  // write packed bcd to memory
  write_virtual_qword(i->seg(), IA32_RMAddr(i),     &save_reg_lo);
  write_virtual_word (i->seg(), IA32_RMAddr(i) + 8, &save_reg_hi);
  the_i387.FPU_pop();
}

/* DF /1 */
void IA32_CPU::FISTTP16(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* DB /1 */
void IA32_CPU::FISTTP32(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}

/* DD /1 */
void IA32_CPU::FISTTP64(Ia32_Instruction_c *i)
{
  UndefinedOpcode(i);
}
